/*
 * Copyright (c) 2024 Black Sesame Technologies
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "dal/allocator.h"
#include "dal/config_loader.h"
#include "dal_rticonnextdds/converter/StatusConverter.hpp"
#include "dal_rticonnextdds/converter/StatusMaskConverter.hpp"
#include "dal_rticonnextdds/converter/RetTypeConverter.hpp"
#include "dal_rticonnextdds/converter/QosConverter.hpp"
#include "dal_rticonnextdds/converter/RetTypeConverter.hpp"
#include "dal_rticonnextdds/converter/SampleInfoConverter.hpp"
#include "dal_rticonnextdds/converter/GuidConverter.hpp"
#include "dal_rticonnextdds/listener_impl/SubscriberListenerImpl.hpp"
#include "dal_rticonnextdds/listener_impl/DataReaderListenerImpl.hpp"
#include "dal_rticonnextdds/utils/log.hpp"
#include "dal_rticonnextdds/utils/EntityGetter.hpp"
#include "dal_rticonnextdds/type_support/RtiTypeSupportImpl.hpp"
#include "dal_rticonnextdds/utils/EntityMappingManager.hpp"

#include "dal_connextdds/type_support.hpp"
#include "dal_connextdds/typecode.hpp"
#include "dal_connextdds/dds_api.hpp"

// SUBSCRIBER
dal_subscriber_listener_t*
dal_create_subscriber_listener(dal_subscriber_listener_callbacks_t callbacks)
{
    dal_subscriber_listener_t* listener = dal_subscriber_listener_allocate();
    if (listener == nullptr) {
        return nullptr;
    }
    listener->handle_ = new SubscriberListenerImpl(callbacks);
    if (!listener->handle_) {
        LOG_ERROR() << "SubscriberListenerImpl memory allocation failed." << LOG_ENDL;
        dal_subscriber_listener_free(listener);
        return nullptr;
    }

    return listener;
}

dal_ret_t dal_delete_subscriber_listener(dal_subscriber_listener_t* listener)
{
    if (!listener || !listener->handle_) {
        LOG_ERROR_WITH_LOCATION() << "listener is invalid." << LOG_ENDL;
        return DAL_RETCODE_BAD_PARAMETER;
    }

    SubscriberListenerImpl* listener_impl =
        reinterpret_cast<SubscriberListenerImpl*>(listener->handle_);
    if (listener_impl->ref_count_.Get() > 0) {
        LOG_ERROR() << "can't delete subscriber listener because it's entity still exist."
                    << LOG_ENDL;
        return DAL_RETCODE_PRECONDITION_NOT_MET;
    }

    delete listener_impl;
    dal_subscriber_listener_free(listener);
    reinterpret_cast<dal_subscriber_listener_t*>(listener)->handle_ = nullptr;

    return DAL_RETCODE_OK;
}

dal_subscriber_t* dal_domain_participant_create_subscriber(dal_domain_participant_t* participant,
                                                           dal_subscriber_qos_t* qos,
                                                           dal_subscriber_listener_t* listener,
                                                           dal_status_mask_t mask)
{
    dal_ret_t ret = DAL_RETCODE_OK;

    DDS_DomainParticipant* r_participant = get_rtidds_domain_participant(participant);
    if (!r_participant) {
        LOG_ERROR_WITH_LOCATION() << "get rtidds participant failed." << LOG_ENDL;
        return nullptr;
    }

    dal_subscriber_t* d_sub = dal_subscriber_allocate();
    if (!d_sub) {
        LOG_ERROR_WITH_LOCATION() << "dal subscriber allocate failed." << LOG_ENDL;
        return nullptr;
    }

    DDS_SubscriberQos* r_qos = new DDS_SubscriberQos();
    DDS_DomainParticipant_get_default_subscriber_qos(r_participant, r_qos);
    ret = QosConverter::GetInstance()->ConvertSubscriberQosFromDalToRtidds(qos, r_qos);
    if (DAL_RETCODE_OK != ret) {
        LOG_ERROR_WITH_LOCATION() << "convert subscriber qos failed." << LOG_ENDL;
        dal_subscriber_free(d_sub);
        return nullptr;
    }

    DDS_StatusMask r_mask = DDS_STATUS_MASK_NONE;
    ret = StatusMaskConverter::GetInstance()->ConvertStatusMaskFromDalToRtidds(mask, &r_mask);
    if (DAL_RETCODE_OK != ret) {
        LOG_ERROR_WITH_LOCATION() << "convert status mask failed." << LOG_ENDL;
        dal_subscriber_free(d_sub);
        return nullptr;
    }

    SubscriberListenerImpl* r_listener_impl =
        listener ? reinterpret_cast<SubscriberListenerImpl*>(listener->handle_) : nullptr;

    DDS_Subscriber* r_sub = DDS_DomainParticipant_create_subscriber(
        r_participant, r_qos, r_listener_impl ? r_listener_impl->getListener() : NULL, r_mask);

    delete r_qos;
    if (!r_sub) {
        LOG_ERROR_WITH_LOCATION() << "create subscriber failed." << LOG_ENDL;
        dal_subscriber_free(d_sub);
        return nullptr;
    }

    EntityMappingAddSubscriber(r_sub, d_sub);
    d_sub->handle_ = new DalSubscriberImpl(r_sub, r_listener_impl);
    return d_sub;
}

dal_subscriber_t*
dal_domain_participant_create_subscriber_with_profile(dal_domain_participant_t* participant,
                                                      dal_subscriber_qos_t* qos,
                                                      const char* profile_name,
                                                      dal_subscriber_listener_t* listener,
                                                      dal_status_mask_t mask)
{
    if (!profile_name || strlen(profile_name) == 0) {
        return dal_domain_participant_create_subscriber(participant, qos, listener, mask);
    }

    dal_ret_t ret = DAL_RETCODE_OK;

    DDS_DomainParticipant* r_participant = get_rtidds_domain_participant(participant);
    if (!r_participant) {
        LOG_ERROR_WITH_LOCATION() << "get rtidds participant failed." << LOG_ENDL;
        return nullptr;
    }

    dal_subscriber_t* d_sub = dal_subscriber_allocate();
    if (!d_sub) {
        LOG_ERROR_WITH_LOCATION() << "dal subscriber allocate failed." << LOG_ENDL;
        return nullptr;
    }

    DDS_DomainParticipantFactory* factory = DDS_DomainParticipantFactory_get_instance();
    DDS_SubscriberQos* r_qos = new DDS_SubscriberQos();
    DDS_ReturnCode_t r_ret = DDS_DomainParticipantFactory_get_subscriber_qos_from_profile(
        factory, r_qos, "dal_rticonnext_qos_profile_lib", profile_name);
    if (r_ret != DDS_RETCODE_OK) {
        ret = QosConverter::GetInstance()->ConvertSubscriberQosFromDalToRtidds(qos, r_qos);
        if (DAL_RETCODE_OK != ret) {
            LOG_ERROR_WITH_LOCATION() << "convert subscriber qos failed." << LOG_ENDL;
            dal_subscriber_free(d_sub);
            return nullptr;
        }
    }

    DDS_StatusMask r_mask = DDS_STATUS_MASK_NONE;
    ret = StatusMaskConverter::GetInstance()->ConvertStatusMaskFromDalToRtidds(mask, &r_mask);
    if (DAL_RETCODE_OK != ret) {
        LOG_ERROR_WITH_LOCATION() << "convert status mask failed." << LOG_ENDL;
        dal_subscriber_free(d_sub);
        return nullptr;
    }

    SubscriberListenerImpl* r_listener_impl =
        listener ? reinterpret_cast<SubscriberListenerImpl*>(listener->handle_) : nullptr;

    DDS_Subscriber* r_sub = DDS_DomainParticipant_create_subscriber(
        r_participant, r_qos, r_listener_impl ? r_listener_impl->getListener() : NULL, r_mask);

    delete r_qos;
    if (!r_sub) {
        LOG_ERROR_WITH_LOCATION() << "create subscriber failed." << LOG_ENDL;
        dal_subscriber_free(d_sub);
        return nullptr;
    }

    EntityMappingAddSubscriber(r_sub, d_sub);
    d_sub->handle_ = new DalSubscriberImpl(r_sub, r_listener_impl);
    return d_sub;
}

dal_ret_t dal_domain_participant_delete_subscriber(dal_domain_participant_t* participant,
                                                   dal_subscriber_t* sub)
{
    DDS_DomainParticipant* r_participant = get_rtidds_domain_participant(participant);
    if (!r_participant) {
        LOG_ERROR_WITH_LOCATION() << "get rtidds participant failed." << LOG_ENDL;
        return DAL_RETCODE_BAD_PARAMETER;
    }

    DDS_Subscriber* r_sub = get_rtidds_subscriber(sub);
    if (!r_sub) {
        LOG_ERROR_WITH_LOCATION() << "get rtidds subscriber failed." << LOG_ENDL;
        return DAL_RETCODE_BAD_PARAMETER;
    }

    DDS_ReturnCode_t ret = DDS_DomainParticipant_delete_subscriber(r_participant, r_sub);
    if (ret != DDS_RETCODE_OK) {
        LOG_ERROR_WITH_LOCATION() << "delete subscriber failed." << LOG_ENDL;
        return DAL_RETCODE_ERROR;
    }

    EntityMappingRemoveSubscriber(r_sub);

    auto sub_impl = reinterpret_cast<DalSubscriberImpl*>(sub->handle_);
    delete sub_impl;
    dal_subscriber_free(sub);
    reinterpret_cast<dal_subscriber_t*>(sub)->handle_ = nullptr;

    return DAL_RETCODE_OK;
}

// DATAREADER
dal_datareader_listener_t*
dal_create_datareader_listener(dal_datareader_listener_callbacks_t callbacks)
{
    dal_datareader_listener_t* listener = dal_datareader_listener_allocate();
    if (listener == nullptr) {
        return nullptr;
    }
    listener->handle_ = (new DataReaderListenerImpl(callbacks));
    if (!listener->handle_) {
        LOG_ERROR() << "DataReaderListenerImpl memory allocation failed." << LOG_ENDL;
        dal_datareader_listener_free(listener);
        return nullptr;
    }

    return listener;
}

dal_ret_t dal_delete_datareader_listener(dal_datareader_listener_t* listener)
{
    if (!listener || !listener->handle_) {
        LOG_ERROR_WITH_LOCATION() << "listener is invalid." << LOG_ENDL;
        return DAL_RETCODE_BAD_PARAMETER;
    }

    DataReaderListenerImpl* listener_impl =
        reinterpret_cast<DataReaderListenerImpl*>(listener->handle_);
    if (listener_impl->ref_count_.Get() > 0) {
        LOG_ERROR() << "can't delete datareader listener because it's entity still exist."
                    << LOG_ENDL;
        return DAL_RETCODE_PRECONDITION_NOT_MET;
    }

    delete listener_impl;
    dal_datareader_listener_free(listener);
    reinterpret_cast<dal_datareader_listener_t*>(listener)->handle_ = nullptr;

    return DAL_RETCODE_OK;
}

static void optimize_datareader_qos_for_large_data(DDS_DataReaderQos* qos)
{
    qos->protocol.rtps_reliable_reader.min_heartbeat_response_delay = DDS_DURATION_ZERO;
    qos->protocol.rtps_reliable_reader.max_heartbeat_response_delay = DDS_DURATION_ZERO;

    qos->reader_resource_limits.dynamically_allocate_fragmented_samples = DDS_BOOLEAN_TRUE;
}

dal_datareader_t* dal_subscriber_create_datareader(dal_subscriber_t* sub,
                                                   dal_topic_description_t* topic_desc,
                                                   dal_datareader_qos_t* qos,
                                                   dal_datareader_listener_t* listener,
                                                   dal_status_mask_t mask)
{
    dal_ret_t d_ret = DAL_RETCODE_OK;

    DDS_Subscriber* r_sub = get_rtidds_subscriber(sub);
    if (!r_sub) {
        LOG_ERROR_WITH_LOCATION() << "get rtidds subscriber failed." << LOG_ENDL;
        return nullptr;
    }

    DDS_Topic* r_topic = get_rtidds_topic_desc(topic_desc);
    if (!r_topic) {
        LOG_ERROR_WITH_LOCATION() << "get rtidds topic failed." << LOG_ENDL;
        return nullptr;
    }

    if (!topic_desc->type_support_impl_) {
        LOG_ERROR_WITH_LOCATION() << "topic description type support is nullptr." << LOG_ENDL;
        return nullptr;
    }

    dal_datareader_t* d_reader = dal_datareader_allocate();
    if (!d_reader) {
        LOG_ERROR_WITH_LOCATION() << "dal datareader allocate failed." << LOG_ENDL;
        return nullptr;
    }

    d_reader->type_support_impl_ = topic_desc->type_support_impl_;

    DDS_DataReaderQos* r_qos = new DDS_DataReaderQos();
    DDS_Subscriber_get_default_datareader_qos(r_sub, r_qos);
    d_ret = QosConverter::GetInstance()->ConvertDataReaderQosFromDalToRtidds(qos, r_qos);
    if (DAL_RETCODE_OK != d_ret) {
        LOG_ERROR_WITH_LOCATION() << "convert datareader qos failed." << LOG_ENDL;
        dal_datareader_free(d_reader);
        return nullptr;
    }
    // if type is unbound set property to force allocation of samples from heap
    const RtiTypeSupportImpl* tsImpl =
        reinterpret_cast<const RtiTypeSupportImpl*>(d_reader->type_support_impl_);
    DAL_Connext_MessageTypeSupport* ts = tsImpl->cxtMsgTs_;
    if (ts->unbounded()) {
        DDS_PropertyQosPolicyHelper_add_property(
            &r_qos->property,
            "dds.data_reader.history.memory_manager.fast_pool.pool_buffer_max_size",
            "0",
            DDS_BOOLEAN_FALSE);
    }
    // optimize when sending large data
    const int LARGE_DATA_SER_SIZE = 1048576;
    if (ts->type_serialized_size_max() >= LARGE_DATA_SER_SIZE) {
        optimize_datareader_qos_for_large_data(r_qos);
    }

    DDS_StatusMask r_mask = DDS_STATUS_MASK_NONE;
    d_ret = StatusMaskConverter::GetInstance()->ConvertStatusMaskFromDalToRtidds(mask, &r_mask);
    if (DAL_RETCODE_OK != d_ret) {
        LOG_ERROR_WITH_LOCATION() << "convert datareader qos failed." << LOG_ENDL;
        dal_datareader_free(d_reader);
        return nullptr;
    }

    DataReaderListenerImpl* r_listener_impl =
        listener ? reinterpret_cast<DataReaderListenerImpl*>(listener->handle_) : nullptr;

    DDS_TopicDescription* topicDesc = DDS_Topic_as_topicdescription(r_topic);
    DDS_DataReader* r_reader = DDS_Subscriber_create_datareader(
        r_sub, topicDesc, r_qos, r_listener_impl ? r_listener_impl->getListener() : NULL, r_mask);

    delete r_qos;
    if (!r_reader) {
        LOG_ERROR_WITH_LOCATION() << "create datareader failed." << LOG_ENDL;
        dal_datareader_free(d_reader);
        return nullptr;
    }

    EntityMappingAddDataReader(r_reader, d_reader);
    d_reader->handle_ = new DalDataReaderImpl(r_reader, r_listener_impl);
    return d_reader;
}

dal_datareader_t* dal_subscriber_create_datareader_with_profile(dal_subscriber_t* sub,
                                                                dal_topic_description_t* topic_desc,
                                                                dal_datareader_qos_t* qos,
                                                                const char* profile_name,
                                                                dal_datareader_listener_t* listener,
                                                                dal_status_mask_t mask)
{
    if (!profile_name || strlen(profile_name) == 0) {
        return dal_subscriber_create_datareader(sub, topic_desc, qos, listener, mask);
    }

    dal_ret_t d_ret = DAL_RETCODE_OK;

    DDS_Subscriber* r_sub = get_rtidds_subscriber(sub);
    if (!r_sub) {
        LOG_ERROR_WITH_LOCATION() << "get rtidds subscriber failed." << LOG_ENDL;
        return nullptr;
    }

    DDS_Topic* r_topic = get_rtidds_topic_desc(topic_desc);
    if (!r_topic) {
        LOG_ERROR_WITH_LOCATION() << "get rtidds topic failed." << LOG_ENDL;
        return nullptr;
    }

    if (!topic_desc->type_support_impl_) {
        LOG_ERROR_WITH_LOCATION() << "topic description type support is nullptr." << LOG_ENDL;
        return nullptr;
    }

    dal_datareader_t* d_reader = dal_datareader_allocate();
    if (!d_reader) {
        LOG_ERROR_WITH_LOCATION() << "dal datareader allocate failed." << LOG_ENDL;
        return nullptr;
    }

    d_reader->type_support_impl_ = topic_desc->type_support_impl_;

    DDS_DomainParticipantFactory* factory = DDS_DomainParticipantFactory_get_instance();
    DDS_DataReaderQos* r_qos = new DDS_DataReaderQos();
    DDS_ReturnCode_t r_ret = DDS_DomainParticipantFactory_get_datareader_qos_from_profile(
        factory, r_qos, "dal_rticonnext_qos_profile_lib", profile_name);
    if (r_ret != DDS_RETCODE_OK) {
        d_ret = QosConverter::GetInstance()->ConvertDataReaderQosFromDalToRtidds(qos, r_qos);
        if (DAL_RETCODE_OK != d_ret) {
            LOG_ERROR_WITH_LOCATION() << "convert datareader qos failed." << LOG_ENDL;
            dal_datareader_free(d_reader);
            return nullptr;
        }
    }

    // if type is unbound set property to force allocation of samples from heap
    const RtiTypeSupportImpl* tsImpl =
        reinterpret_cast<const RtiTypeSupportImpl*>(d_reader->type_support_impl_);
    DAL_Connext_MessageTypeSupport* ts = tsImpl->cxtMsgTs_;
    if (ts->unbounded()) {
        DDS_PropertyQosPolicyHelper_add_property(
            &r_qos->property,
            "dds.data_reader.history.memory_manager.fast_pool.pool_buffer_max_size",
            "0",
            DDS_BOOLEAN_FALSE);
    }
    // optimize when sending large data
    const int LARGE_DATA_SER_SIZE = 1048576;
    if (ts->type_serialized_size_max() >= LARGE_DATA_SER_SIZE) {
        optimize_datareader_qos_for_large_data(r_qos);
    }

    DDS_StatusMask r_mask = DDS_STATUS_MASK_NONE;
    d_ret = StatusMaskConverter::GetInstance()->ConvertStatusMaskFromDalToRtidds(mask, &r_mask);
    if (DAL_RETCODE_OK != d_ret) {
        LOG_ERROR_WITH_LOCATION() << "convert datareader qos failed." << LOG_ENDL;
        dal_datareader_free(d_reader);
        return nullptr;
    }

    DataReaderListenerImpl* r_listener_impl =
        listener ? reinterpret_cast<DataReaderListenerImpl*>(listener->handle_) : nullptr;

    DDS_TopicDescription* topicDesc = DDS_Topic_as_topicdescription(r_topic);
    DDS_DataReader* r_reader = DDS_Subscriber_create_datareader(
        r_sub, topicDesc, r_qos, r_listener_impl ? r_listener_impl->getListener() : NULL, r_mask);

    delete r_qos;
    if (!r_reader) {
        LOG_ERROR_WITH_LOCATION() << "create datareader failed." << LOG_ENDL;
        dal_datareader_free(d_reader);
        return nullptr;
    }

    EntityMappingAddDataReader(r_reader, d_reader);
    d_reader->handle_ = new DalDataReaderImpl(r_reader, r_listener_impl);
    return d_reader;
}

dal_ret_t dal_subscriber_delete_datareader(dal_subscriber_t* sub, dal_datareader_t* reader)
{
    DDS_Subscriber* r_sub = get_rtidds_subscriber(sub);
    if (!r_sub) {
        LOG_ERROR_WITH_LOCATION() << "get rtidds subscriber failed." << LOG_ENDL;
        return DAL_RETCODE_BAD_PARAMETER;
    }

    DDS_DataReader* r_reader = get_rtidds_datareader(reader);
    if (!r_reader) {
        LOG_ERROR_WITH_LOCATION() << "get rtidds datareader failed." << LOG_ENDL;
        return DAL_RETCODE_BAD_PARAMETER;
    }

    EntityMappingRemoveDataReader(r_reader);

    DDS_ReturnCode_t ret = DDS_Subscriber_delete_datareader(r_sub, r_reader);
    if (ret != DDS_RETCODE_OK) {
        LOG_ERROR_WITH_LOCATION() << "delete datareader failed." << LOG_ENDL;
        return DAL_RETCODE_ERROR;
    }

    auto reader_impl = reinterpret_cast<DalDataReaderImpl*>(reader->handle_);
    delete reader_impl;
    dal_datareader_free(reader);
    reinterpret_cast<dal_datareader_t*>(reader)->handle_ = nullptr;
    return DAL_RETCODE_OK;
}

dal_ret_t dal_datareader_read_next_sample(dal_datareader_t* reader,
                                          dal_data data,
                                          dal_sample_info_t* d_sample_info)
{
    DDS_DataReader* r_reader = get_rtidds_datareader(reader);
    if (!r_reader) {
        LOG_ERROR_WITH_LOCATION() << "get rtidds datareader failed." << LOG_ENDL;
        return DAL_RETCODE_BAD_PARAMETER;
    }

    const RtiTypeSupportImpl* tsImpl =
        reinterpret_cast<const RtiTypeSupportImpl*>(reader->type_support_impl_);
    DAL_Connext_MessageTypeSupport* ts = tsImpl->cxtMsgTs_;

    void* ros_message = data;
    void** data_buffer = nullptr;
    DDS_SampleInfoSeq loan_info = DDS_SEQUENCE_INITIALIZER;
    DDS_Boolean is_loan = DDS_BOOLEAN_TRUE;
    DDS_Long data_len = 0;
    DAL_Connext_UntypedSampleSeq loan_data;

    DDS_ReturnCode_t r_ret = DDS_DataReader_read_or_take_instance_untypedI(r_reader,
                                                                           &is_loan,
                                                                           &data_buffer,
                                                                           &data_len,
                                                                           &loan_info,
                                                                           0,
                                                                           0,
                                                                           DDS_BOOLEAN_TRUE,
                                                                           NULL,
                                                                           1,
                                                                           1,
                                                                           &DDS_HANDLE_NIL,
                                                                           NULL,
                                                                           DDS_ANY_SAMPLE_STATE,
                                                                           DDS_ANY_VIEW_STATE,
                                                                           DDS_ANY_INSTANCE_STATE,
                                                                           DDS_BOOLEAN_FALSE);
    if (DDS_RETCODE_NO_DATA == r_ret) {
        return DAL_RETCODE_NO_DATA;
    }
    if (DDS_RETCODE_OK != r_ret) {
        LOG_ERROR_WITH_LOCATION() << "read_or_take_instance_untypedI fail." << LOG_ENDL;
        return DAL_RETCODE_ERROR;
    }

    if (data_len > 0) {
        DAL_Connext_MessagePtrSeq_loan_contiguous(
            &loan_data, reinterpret_cast<DAL_Connext_Message**>(data_buffer), data_len, data_len);
    }

    DAL_Connext_Message* msg = reinterpret_cast<DAL_Connext_Message*>(
        DDS_UntypedSampleSeq_get_reference(&loan_data, static_cast<uint32_t>(0)));
    DDS_SampleInfo* r_sampleInfo =
        DDS_SampleInfoSeq_get_reference(&loan_info, static_cast<uint32_t>(0));

    size_t deserialized_size = 0;
    if (r_sampleInfo->valid_data) {
        ts->deserialize(ros_message, &msg->data_buffer, deserialized_size);
    }

    // return samples
    void** data_buffer_2 =
        reinterpret_cast<void**>(DAL_Connext_MessagePtrSeq_get_contiguous_buffer(&loan_data));
    const DDS_Long data_len_2 = DAL_Connext_MessagePtrSeq_get_length(&loan_data);
    if (!DAL_Connext_MessagePtrSeq_unloan(&loan_data)) {
        LOG_ERROR_WITH_LOCATION() << "failed to unloan sample sequence." << LOG_ENDL;
        return DAL_RETCODE_ERROR;
    }

    if (DDS_RETCODE_OK !=
        DDS_DataReader_return_loan_untypedI(r_reader, data_buffer_2, data_len_2, &loan_info)) {
        LOG_ERROR_WITH_LOCATION() << "failed to return loan to DDS reader." << LOG_ENDL;
        return DAL_RETCODE_ERROR;
    }

    if (r_ret == DDS_RETCODE_OK && r_sampleInfo->valid_data) {
        SampleInfoConverter::GetInstance()->ConvertSampleInfoFromRtiddsToDal(d_sample_info,
                                                                             r_sampleInfo);
        return DAL_RETCODE_OK;
    }
    return RetTypeConverter::GetInstance()->ConvertRetFromRtiddsToDal(r_ret);
}

dal_ret_t dal_datareader_take_next_sample(dal_datareader_t* reader,
                                          dal_data data,
                                          dal_sample_info_t* d_sample_info)
{
    DDS_DataReader* r_reader = get_rtidds_datareader(reader);
    if (!r_reader) {
        LOG_ERROR_WITH_LOCATION() << "get rtidds datareader failed." << LOG_ENDL;
        return DAL_RETCODE_BAD_PARAMETER;
    }

    const RtiTypeSupportImpl* tsImpl =
        reinterpret_cast<const RtiTypeSupportImpl*>(reader->type_support_impl_);
    DAL_Connext_MessageTypeSupport* ts = tsImpl->cxtMsgTs_;

    void* ros_message = data;
    void** data_buffer = nullptr;
    DDS_SampleInfoSeq loan_info = DDS_SEQUENCE_INITIALIZER;
    DDS_Boolean is_loan = DDS_BOOLEAN_TRUE;
    DDS_Long data_len = 0;
    DAL_Connext_UntypedSampleSeq loan_data;

    DDS_ReturnCode_t r_ret = DDS_DataReader_read_or_take_instance_untypedI(r_reader,
                                                                           &is_loan,
                                                                           &data_buffer,
                                                                           &data_len,
                                                                           &loan_info,
                                                                           0,
                                                                           0,
                                                                           DDS_BOOLEAN_TRUE,
                                                                           NULL,
                                                                           1,
                                                                           1,
                                                                           &DDS_HANDLE_NIL,
                                                                           NULL,
                                                                           DDS_ANY_SAMPLE_STATE,
                                                                           DDS_ANY_VIEW_STATE,
                                                                           DDS_ANY_INSTANCE_STATE,
                                                                           DDS_BOOLEAN_TRUE);
    if (DDS_RETCODE_NO_DATA == r_ret) {
        return DAL_RETCODE_NO_DATA;
    }
    if (DDS_RETCODE_OK != r_ret) {
        LOG_ERROR_WITH_LOCATION() << "read_or_take_instance_untypedI fail." << LOG_ENDL;
        return DAL_RETCODE_ERROR;
    }

    if (data_len > 0) {
        DAL_Connext_MessagePtrSeq_loan_contiguous(
            &loan_data, reinterpret_cast<DAL_Connext_Message**>(data_buffer), data_len, data_len);
    }

    DAL_Connext_Message* msg = reinterpret_cast<DAL_Connext_Message*>(
        DDS_UntypedSampleSeq_get_reference(&loan_data, static_cast<uint32_t>(0)));
    DDS_SampleInfo* r_sampleInfo =
        DDS_SampleInfoSeq_get_reference(&loan_info, static_cast<uint32_t>(0));

    size_t deserialized_size = 0;
    if (r_sampleInfo->valid_data) {
        ts->deserialize(ros_message, &msg->data_buffer, deserialized_size);
    }

    // return samples
    void** data_buffer_2 =
        reinterpret_cast<void**>(DAL_Connext_MessagePtrSeq_get_contiguous_buffer(&loan_data));
    const DDS_Long data_len_2 = DAL_Connext_MessagePtrSeq_get_length(&loan_data);
    if (!DAL_Connext_MessagePtrSeq_unloan(&loan_data)) {
        LOG_ERROR_WITH_LOCATION() << "failed to unloan sample sequence." << LOG_ENDL;
        return DAL_RETCODE_ERROR;
    }

    if (DDS_RETCODE_OK !=
        DDS_DataReader_return_loan_untypedI(r_reader, data_buffer_2, data_len_2, &loan_info)) {
        LOG_ERROR_WITH_LOCATION() << "failed to return loan to DDS reader." << LOG_ENDL;
        return DAL_RETCODE_ERROR;
    }

    if (r_ret == DDS_RETCODE_OK && r_sampleInfo->valid_data) {
        SampleInfoConverter::GetInstance()->ConvertSampleInfoFromRtiddsToDal(d_sample_info,
                                                                             r_sampleInfo);
        return DAL_RETCODE_OK;
    }
    return RetTypeConverter::GetInstance()->ConvertRetFromRtiddsToDal(r_ret);
}

dal_guid_t dal_datareader_get_guid(dal_datareader_t* reader)
{
    dal_guid_t d_guid = { };
    DDS_GUID_t r_guid;

    DDS_DataReader* r_reader = get_rtidds_datareader(reader);
    if (!r_reader) {
        LOG_ERROR_WITH_LOCATION() << "get rtidds datareader failed." << LOG_ENDL;
        return d_guid;
    }
    DDS_InstanceHandle_t r_ins_handle =
        DDS_Entity_get_instance_handle(DDS_DomainParticipant_as_entity(r_reader));

    memcpy(r_guid.value, r_ins_handle.keyHash.value, DDS_GUID_LENGTH);
    GuidConverter::GetInstance()->ConvertGuidFromRtiddsToDal(&r_guid, &d_guid);
    return d_guid;
}
